hip.math.quaternion source code

1 module hip.math.quaternion;
2 import core.stdc.math : sin,  cos, sqrt;
3 import hip.math.vector;
4 import hip.math.matrix;
5 
6 struct Quaternion
7 {
8     union {
9         struct {float x, y, z, w;}
10         float[4] values;
11     }
12 
13     ///https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
14     static rotation(float angle, in Vector3 axis)
15     {
16         float c = cos(angle/2);
17         float s = sin(angle/2);
18         Vector3 a = axis.unit;
19 
20         return Quaternion(s*a.x, s*a.y, s*a.z, c);
21     }
22 
23     Quaternion opUnary(string op)() if(op == "-")
24     {
25         return Quaternion(-x, -y, -z, w);
26     }
27 
28     Quaternion unit()
29     {
30         float magnitude = mag();
31         if(magnitude == 0)
32             return Quaternion(0,0,0,0);
33         return Quaternion(x/magnitude, y/magnitude, z/magnitude, w/magnitude);
34     }
35     void normalize()
36     {
37         float magnitude = mag();
38         if(magnitude == 0)
39             values[] = 0;
40         else
41             values[]/= magnitude;
42     }
43     
44 
45 
46     Quaternion opBinary(string op)(in Quaternion rhs) const
47     {
48         static if(op == "*")
49         {
50             Quaternion ret;
51             ret.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z;
52             ret.x = w*rhs.x + x*rhs.w + y*rhs.z - z*rhs.y;
53 	        ret.y = w * rhs.y + y * rhs.w + z * rhs.x - x * rhs.z;
54 	        ret.z = w * rhs.z + z * rhs.w + x * rhs.y - y * rhs.x;
55             return ret.unit;
56         }
57     }
58 
59     Matrix3 toMatrix3()
60     {
61         float rcos = cos(w);
62         float rsin = sin(w);
63         float dcos = 1 - rcos;
64         Matrix3 matrix;
65         matrix[0,0] =      rcos + x*x*dcos;
66         matrix[1,0] =  z * rsin + y*x*dcos;
67         matrix[2,0] = -y * rsin + z*x*dcos;
68         matrix[0,1] = -z * rsin + x*y*dcos;
69         matrix[1,1] =      rcos + y*y*dcos;
70         matrix[2,1] =  x * rsin + z*y*dcos;
71         matrix[0,2] =  y * rsin + x*z*dcos;
72         matrix[1,2] = -x * rsin + y*z*dcos;
73         matrix[2,2] =      rcos + z*z*dcos;
74         return matrix;
75     }
76 
77     Matrix4 toMatrix4()
78     {
79         float rcos = cos(w);
80         float rsin = sin(w);
81         float dcos = 1 - rcos;
82         Matrix4 matrix;
83         matrix[0,0] =      rcos + x*x*dcos;
84         matrix[1,0] =  z * rsin + y*x*dcos;
85         matrix[2,0] = -y * rsin + z*x*dcos;
86         matrix[0,1] = -z * rsin + x*y*dcos;
87         matrix[1,1] =      rcos + y*y*dcos;
88         matrix[2,1] =  x * rsin + z*y*dcos;
89         matrix[0,2] =  y * rsin + x*z*dcos;
90         matrix[1,2] = -x * rsin + y*z*dcos;
91         matrix[2,2] =      rcos + z*z*dcos;
92         matrix[3,0] = 0;
93         matrix[3,1] = 0;
94         matrix[3,2] = 0;
95         matrix[3,3] = 1;
96 
97         return matrix;
98     }
99 
100     float mag(){return sqrt(x*x+y*y+z*z+w*w);}
101     alias values this;
102 }